#include "lib/WrapFuncs.h"
#include "/usr/include/math.h"
#include <vector>
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
    namespace Funcs
    {

        double DistToLine(PoseF inlier1, PoseF inlier2, PoseF outlier)
        {
            double l1[2] = {inlier2.x(), inlier2.y()};
            double l2[2] = {inlier1.x(), inlier1.y()};
            double o3[2] = {outlier.x(), outlier.y()};
            Eigen::Vector4f vec1(l2[0] - l1[0], l2[1] - l1[1], 0, 0);
            vec1.normalize();
            Eigen::Vector4f vec2(o3[0] - l2[0], o3[1] - l2[1], 0, 0);
            double dst = vec1.cross3(vec2).squaredNorm();
            return dst;
        }

        double DistToLine(double line_k, double line_b, PoseF outlier)
        {
            double dst_abs = std::fabs(line_k * outlier.GetPoint().x() - outlier.GetPoint().y() + line_b);
            double dst_sqrt = std::sqrt(line_k * line_k + 1.0);
            return (dst_abs / dst_sqrt);
        }
        
        double DistToPoint(PoseF point_1st, PoseF point_2nd)
        {
            double x_diff = point_1st.x() - point_2nd.x();
            double y_diff = point_1st.y() - point_2nd.y();
            double dst = x_diff * x_diff + y_diff * y_diff;
            return dst;
        }

        void GetInlier(const double &line_a, const double &line_b, const double &line_c,
                       const PoseF &fit_point1, const PoseF &fit_point2,
                       PoseF &start_point, PoseF &end_point)
        {
            double line_a2 = line_a * line_a;
            double line_b2 = line_b * line_b;
            double line_a2_b2 = line_a2 + line_b2;
            double line_ab = line_a * line_b;
            double line_ac = line_a * line_c;
            double line_bc = line_b * line_c;

            start_point.x() = (line_b2 * fit_point1.x() - line_ab * fit_point1.y() - line_ac) / line_a2_b2;
            start_point.y() = (-line_ab * fit_point1.x() + line_a2 * fit_point1.y() - line_bc) / line_a2_b2;
            end_point.x() = (line_b2 * fit_point2.x() - line_ab * fit_point2.y() - line_ac) / line_a2_b2;
            end_point.y() = (-line_ab * fit_point2.x() + line_a2 * fit_point2.y() - line_bc) / line_a2_b2;
        }

        void GetInlier(const double &line_a, const double &line_b, const double &line_c,
                       const PoseF &fit_point,PoseF &result_point)
        {
            double line_a2 = line_a * line_a;
            double line_b2 = line_b * line_b;
            double line_a2_b2 = line_a2 + line_b2;
            double line_ab = line_a * line_b;
            double line_ac = line_a * line_c;
            double line_bc = line_b * line_c;

            result_point.x() = (line_b2 * fit_point.x() - line_ab * fit_point.y() - line_ac) / line_a2_b2;
            result_point.y() = (-line_ab * fit_point.x() + line_a2 * fit_point.y() - line_bc) / line_a2_b2;

        }



        double GetVectorialAngle(PoseF &start_point, PoseF &end_point)
        {
            double vectorA[2] = {1.0, 0.0}, vectorB[2] = {end_point.x() - start_point.x(), end_point.y() - start_point.y()};
            double AB, A1, B1, cosr, angle;
            AB = vectorA[0] * vectorB[0] + vectorA[1] * vectorB[1];
            A1 = sqrt(vectorA[0] * vectorA[0] + vectorA[1] * vectorA[1]);
            B1 = sqrt(vectorB[0] * vectorB[0] + vectorB[1] * vectorB[1]);
            cosr = AB / A1 / B1;
            angle = (vectorB[1] / fabs(vectorB[1])) * acos(cosr);
            return angle;
        }

        double NormalizeAngle(double raw_angle)
        {
            if (-M_PI <= raw_angle && raw_angle <= M_PI)
            {
                return raw_angle;
            }
            while(raw_angle> M_PI)  {raw_angle -= M_PI*2;} //dpj
            while(raw_angle< -M_PI) {raw_angle += M_PI*2;}

            return raw_angle;
        }

        void LineFit(PointCloud<double> &world_point_cloud, double &line_k, double &line_b)
        {
            //使用Eigen库构建查找直线
            int pc_size = (int)world_point_cloud.size();
            if (pc_size > 0)
            {
                Eigen::MatrixXd Hk(pc_size, 2);
                Eigen::MatrixXd S(pc_size, 1);
                Eigen::MatrixXd St(1, pc_size);

                for (size_t i = 0; i < pc_size; i++)
                {
                    Hk(i, 0) = world_point_cloud[i].x();
                    Hk(i, 1) = 1;
                    St(0, i) = world_point_cloud[i].y();
                }

                S = St.transpose();

                Eigen::MatrixXd Hkt(2, pc_size);
                Hkt = Hk.transpose();
                Eigen::Matrix<double, 2, 2> Hk_ = Hkt * Hk;
                Eigen::Matrix<double, 2, 1> result = Hk_.inverse() * Hkt * S;

                line_k = result(0, 0);
                line_b = result(1, 0);
            }
            else
            {
                std::cout << "点云为空" << std::endl;
            }
        }

        void LineFit(PointCloud<double> &world_point_cloud, double &line_k, double &line_b,double &r,double &x_mean,double &y_mean)
        {
            //使用Eigen库构建查找直线
            int pc_size = (int)world_point_cloud.size();
            if (pc_size > 0)
            {
                double avgX=0,avgY=0;    //定义变量
                double lxx=0,lyy=0,lxy=0;
                for(int i=0;i<pc_size;i++)   //计算X、Y的平均值
                {
                    avgX+=world_point_cloud[i].x()/pc_size;
                    avgY+=world_point_cloud[i].y()/pc_size;
                }
                for(int i=0;i<pc_size;i++)   //计算Lxx、Lyy和Lxy
                {
                    lxx+=(world_point_cloud[i].x()-avgX)*(world_point_cloud[i].x()-avgX);
                    lyy+=(world_point_cloud[i].y()-avgY)*(world_point_cloud[i].y()-avgX);
                    lxy+=(world_point_cloud[i].x()-avgX)*(world_point_cloud[i].y()-avgY);
                }
                x_mean= avgX;
                y_mean = avgY;
                line_k= lxy/lxx;
                line_b= avgY-lxy*avgX/lxx;
                r= lxy/sqrt(lxx*lyy); //返回相关系数r
                //std::cout<<"This line can be fitted by y=kx+b."<<std::endl;
                //std::cout<<"k= "<<line_k<<std::endl;  //输出回归系数a
                //std::cout<<"b= "<<line_b<<std::endl;   //输出回归系数b
                //std::cout<<"r= "<<r<<std::endl;   //输出回相关系数r

            }
            else
            {
                std::cout << "点云为空" << std::endl;
            }
        }

        void FormScanToPC(const sensor_msgs::msg::LaserScan &msg, PointCloud<double> &dst_pc)
        {
            dst_pc.clear();
            sensor_msgs::msg::PointCloud2 cloud;
            laser_geometry::LaserProjection projector_;
            PointF point_;
            projector_.projectLaser(msg, cloud);

            pcl::PointCloud<pcl::PointXYZ> cloudXYZ;
            pcl::fromROSMsg(cloud, cloudXYZ);
            //std::cout<< "cloudXYZ.height= " << cloudXYZ.height << " cloudXYZ.width= " <<cloudXYZ.width<< std::endl;

            for (size_t i = 0; i < cloudXYZ.height; ++i)
            {
                for (size_t j = 0; j < cloudXYZ.width; ++j)
                {
                    const size_t k = cloudXYZ.width * i + j;
                    //激光雷达数据为激光雷达坐标系下，需要转换到机器人坐标系下
                    point_= lidar_pose.ConvertOnCur(PointF(cloudXYZ.points[k].x, cloudXYZ.points[k].y));
                    dst_pc.emplace_back(point_);
                    //std::cout << cloudXYZ.points[k].x << " " << cloudXYZ.points[k].y << " ";
                }
            }
        }

    } // namespace Funcs

} // namespace behavior_controller